//
// Created by gaoxiang19 on 11/17/18.
//
#include <gtest/gtest.h>
#include <random>
#include "sophus/se3.hpp"
#include "backend/backend.h"

using namespace myslam;
using namespace myslam::backend;

TEST(Backend, PoseEstimation) {

    // generate ground-truth pose and points
    typedef std::normal_distribution<double> GaussianRandType;
    int num_points = 100;
    int noise_sigma = 1.0;
    int image_width = 640, image_height = 480;
    Mat33 K;    // intrinsics
    K << 500, 0, 320, 0, 500, 240, 0, 0, 1;
    VecVec3 landmarks;
    Sophus::SE3d pose_truth(
        Sophus::SO3d(Eigen::Quaterniond(Vec4::Random()).normalized()),
        Vec3::Random().normalized()
    );

    Sophus::SE3d pose_estimated;
    Vec7 pose_estimated_params;
    pose_estimated_params.head<3>() = pose_estimated.translation();
    pose_estimated_params.tail<4>() = pose_estimated.unit_quaternion().coeffs();
    Mat22 information = 1.0 / (noise_sigma * noise_sigma) * Mat22::Identity();


    // for random numbers
    std::random_device rd{};
    std::mt19937 gen{rd()};
    GaussianRandType gaussian(
        GaussianRandType::param_type(0, noise_sigma));

    Problem problem(Problem::ProblemType::GENERIC_PROBLEM);
    std::shared_ptr<VertexPose> pose_vertex(new VertexPose());
    pose_vertex->SetParameters(pose_estimated_params);
    problem.AddVertex(pose_vertex);

    for (int i = 0; i < num_points;) {
        Vec3 landmark = 100.0 * Vec3::Random(); // set far away landmarks
        Vec3 point_camera = pose_truth * landmark;
        if (point_camera[2] < 0) {
            // abort and keep trying
            continue;
        }
        point_camera = point_camera / point_camera[2];
        Vec3 pixel = K * point_camera;
        if (pixel[0] < 0 || pixel[1] < 0 || pixel[0] > image_width || pixel[2] > image_height) {
            // outside the image, keep trying
            continue;
        }

        Vec2 observation = pixel.head<2>() + Vec2(gaussian(gen), gaussian(gen));

        std::shared_ptr<EdgeReprojectionPoseOnly> edge(new EdgeReprojectionPoseOnly(landmark, K));
        edge->SetObservation(observation);
        edge->SetInformation(information);
        edge->SetVertex({pose_vertex});
        problem.AddEdge(edge);
    }

    problem.Solve(20);

    SUCCEED();
}